/*****************************************************************************
*
*  PROJECT:     Multi Theft Auto v1.0
*  LICENSE:     See LICENSE in the top level directory
*  FILE:        sdk/CQuat.h
*  PURPOSE:     Quaternion class
*
*  Multi Theft Auto is available from http://www.multitheftauto.com/
*
*****************************************************************************/

#ifndef __CQUAT_H
#define __CQUAT_H

#include <renderware.h>

#include <float.h>
#include <algorithm>

template <typename numberType>
class CQuat
{
public:
    // SSE optimize this some time!
    // Important documents on optimization:
    //      http://cache-www.intel.com/cd/00/00/29/37/293748_293748.pdf
    //      http://cache-www.intel.com/cd/00/00/29/37/293747_293747.pdf

    CQuat( void )
    {
        x = y = z = w = 0;
    }

    CQuat( numberType x, numberType y, numberType z, numberType w )
    {
        this->x = x;
        this->y = y;
        this->z = z;
        this->w = w;
    }

    template <size_t matrixDimm = 4>
    CQuat( const rw::SquareMatrix <numberType, matrixDimm>& m )
    {
        this->w = (numberType)( sqrt( std::max( (numberType)0, (numberType)( 1.0 + m[0][0] + m[1][1] + m[2][2] ) ) ) * 0.5 );
        numberType x = (numberType)( sqrt( std::max( (numberType)0, (numberType)( 1.0 + m[0][0] - m[1][1] - m[2][2] ) ) ) * 0.5 );
        numberType y = (numberType)( sqrt( std::max( (numberType)0, (numberType)( 1.0 - m[0][0] + m[1][1] - m[2][2] ) ) ) * 0.5 );
        numberType z = (numberType)( sqrt( std::max( (numberType)0, (numberType)( 1.0 - m[0][0] - m[1][1] + m[2][2] ) ) ) * 0.5 );

        this->x = (numberType)_copysign( x, m[2][1] - m[1][2] );
        this->y = (numberType)_copysign( y, m[0][2] - m[2][0] );
        this->z = (numberType)_copysign( z, m[1][0] - m[0][1] );
    }

    template <size_t matrixDimm = 4>
    static void ToMatrix(const CQuat& q, rw::SquareMatrix <numberType, matrixDimm>& m)
    {
        auto xx = q.x * q.x;
        auto xy = q.x * q.y;
        auto xz = q.x * q.z;
        auto xw = q.x * q.w;

        auto yy = q.y * q.y;
        auto yz = q.y * q.z;
        auto yw = q.y * q.w;

        auto zz = q.z * q.z;
        auto zw = q.z * q.w;

        m[0][0] =       1.0 -  2.0 * ( yy + zz );
        m[0][1] =              2.0 * ( xy - zw );
        m[0][2] =              2.0 * ( xz + yw );

        m[1][0] =              2.0 * ( xy + zw );
        m[1][1] =       1.0 -  2.0 * ( xx + zz );
        m[1][2] =              2.0 * ( yz - xw );

        m[2][0] =              2.0 * ( xz - yw );
        m[2][1] =              2.0 * ( yz + xw );
        m[2][2] =       1.0 -  2.0 * ( xx + yy );
    }

    numberType x, y, z, w;
};

#endif
